一种欠驱动机械臂控制装置及控制方法,属于自动控制领域,过程为:欠驱动机械臂控制装置发出电压信号,启动欠驱动机械臂电机;欠驱动机械臂的电机带动主动臂和欠驱动臂摆起,至非完全竖直平衡位置;通过编码器反馈的角度值判断欠驱动臂是否偏离预定的非完全竖直平衡位置,如偏离,则调整欠驱动臂回到预定平衡位置,并保持维持该状态按计算出的欠驱动机械臂的电机转矩调整电机,使摆臂调整回预定的非完全竖直平衡位置,本发明实现了上位机与嵌入式单板机的Internet连接,其通讯便捷且接口丰富,实现了对被控对象的实时控制,且支持多种语言编译。
1.一种欠驱动机械臂控制装置,包括上位机、AD采集卡、调理模块,调理模块包括信号调理电路、数字IO调理电路、AD信号采样调理电路,其特征在于:所述控制装置还包括嵌入式单板机、计数采集卡、驱动接口模块和被控对象模块,所述的调理模块还包括编码器输入调理电路、PWM输出调理电路,驱动接口模块由电机驱动接口电路组成,所述的被控对象模块包括:编码器、电机、主动臂和欠驱动臂,其连接关系如下:上位机的网卡输入输出端连接嵌入式单板机的网卡输入输出端,嵌入式单板机的第一输入输出端连接计数采集卡的输入输出端,嵌入式单板机的第二输入输出端连接AD采集卡的第一输入输出端,计数采集卡的第一输入端连接第一信号调理电路的输出端,计数采集卡的第二输入端连接第二信号调理电路的输出端,第一信号调理电路的输入端连接编码器输入调理电路的第一输出端,第二信号调理电路的输入端连接编码器输入调理电路的第二输出端,计数采集卡的输出端连接PWM输出调理电路的第一输入端,AD采集卡的输出端连接PWM输出调理电路的第二输入端,AD采集卡的第二输入输出端连接数字IO调理电路的输入输出端,AD采集卡的输入端连接AD信号采样调理电路的输出端,PWM输出调理电路的第一输出端连接第一电机驱动接口电路的输入端,PWM输出调理电路的第二输出端连接第二电机驱动接口电路的输入端;编码器输入调理电路的第一输入端连接第二编码器的输出端,编码器输入调理电路的第二输入端连接第一编码器的输出端,电机驱动接口电路的输出端连接电机的输入端。xa0
2.根据权利要求1所述的欠驱动机械臂控制装置,其特征在于:所述的被控对象模块,电机固定在电机基座上,电机轴的一端连接主动臂的一端,电机轴的另一端连接第一编码器,所述的主动臂与欠驱动臂通过轴承连接,在所述轴承的一端连接所述主动臂的另一端,在所述轴承的另一端连接第二编码器。xa0
3.采用权利要求1所述的欠驱动机械臂控制装置的控制方法,其特征在于:包括以下步骤:xa0步骤1:欠驱动机械臂控制装置发出电压信号,启动欠驱动机械臂的电机,此时,欠驱动机械臂静止于初始位置,所述的初始位置是指主动臂竖直向下,欠驱动臂与主动臂成顺时针180°角;xa0步骤2:调节欠驱动机械臂的电机转矩,使欠驱动机械臂的电机带动主动臂和欠驱动臂摆起,至非完全竖直平衡位置,所述的非完全竖直平衡位置是指主动臂从竖直向下的初始位置沿逆时针方向旋转到达的位置,相应形成的角度,记为q₁,欠驱动臂竖直向上,所述的q₁角的范围为:135°~225°;xa0步骤3:通过编码器反馈的角度值判断欠驱动臂是否偏离预定的非完全竖直平衡位置,xa0如偏离,则调整欠驱动机械臂的电机转矩,使欠驱动臂回到预定平衡位置,并保持维持该状态,所述的欠驱动机械臂的电机所需的转矩为:xa0ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="486"/>其中:xa0ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="661"/>式中,u表示欠驱动机械臂的驱动电机对摆臂的转矩输入;x表示欠驱动机械臂摆臂的角度和角速度状态量;w表示欠驱动机械臂非完全竖直平衡位置的参考角度信号;K_i表示欠驱动机械臂非完全竖直平衡控制调节律的状态反馈增益系数;L_i表示欠驱动机械臂非完全竖直平衡位置的参考角度信号权重调节系数;r表示非完全竖直平衡控制调节律的规则个数;λ_i表示非完全竖直平衡控制调节律的隶属度函数;z表示非完全竖直平衡控制调节律的条件变量集合;xa0步骤4:按步骤3计算出的欠驱动机械臂的电机转矩调整电机,使摆臂调整回预定的非完全竖直平衡位置,所述的预定的非完全竖直平衡位置由使用者自行设定,主动臂与竖直向下方向逆时针所呈夹角为135°~225°,且欠驱动臂竖直向上。xa0
4.根据权利要求3所述的欠驱动机械臂控制装置的控制方法,其特征在于:步骤2所述的欠驱动机械臂电机所需转矩,按照如下公式计算:xa0u=u₁+u₂ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="700"/>其中,xa0E=(θ₄+θ₅)gxa0ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="700"/>ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="235"/>h₂=(θ₄+θ₅)g+δ;xa0u₂=u₂₁+u₂₂其中,xa0ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="685"/>ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="546"/>式中,u表示欠驱动机械臂的驱动电机输出转矩;u₁表示转矩输入的能量分量;u₂表示转矩输入的摩擦补偿分量;q₁表示主动臂从竖直向下的初始位置沿着逆时针方向所成的实际角度值;ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="43"/>表示主动臂从竖直向下的初始位置沿着逆时针方向所成的实际角速度值;q₂表示欠驱动臂相对于主动臂转动的实际角度值;ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="47"/>表示欠驱动臂相对于主动臂转动的实际角速度值;q_d表示主动臂达到非完全竖直平衡位置的目标角度,即与竖直向下位置逆时针方向成195°角;δ表示任意小正数;g表示重力加速度;θ₂,θ₃,θ₄,θ₅表示欠驱动机械臂摆臂常数;h₁,k₀,k₁,k₂,k₃,k₄表示待定正常数;ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="52"/>表示正整数且ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="144"/>μ,ζ表示两个互质正整数且μ<ζ<10,μ和ζ;u₂₁表示神经网络补偿量;u₂₂表示补偿控制量;z_i表示BP神经网络的输入,z_i∈Rⁿ;θ_vj表示BP神经网络输入层的阈值;v_ji表示BP神经网络的输入层到隐含层的权值,v_ji∈R^n×l;ω_j表示BP神经网络隐含层到输出层的权值,ω_j∈Rl;ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="169"/>表示理想的常量权值;ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="165"/>分别表示ntent="drawing" img-format="jpg" inline="no" orientation="portrait" wi="162"/>的估计值;θ_w表示BP神经网络隐含层的阈值;n表示BP神经网络输入层神经元个数;xa0l表示BP神经网络隐含层神经元个数,lxa0≥1;s(.)表示BP神经网络隐含层函数;ε表示BP神经网络的建模误差;σ表示正常数,且σ>|ε|。xa0技术领域
本发明属于自动控制领域,特别涉及一种欠驱动机械臂装置及控制方法。
背景技术
随着控制理论技术和工业技术不断发展,控制理论研究人员需要对复杂被控对象进行先进的、智能的控制算法研究,从而提高工业生产过程的稳定性和操作精度。为了避免资源浪费和安全隐患等不良后果,复杂控制算法需要首先在实验系统上进行有效性验证后,才能应用到实际工业过程中。因此,实验系统对复杂控制方法在实际工业生产中的应用起到至关重要的作用。目前的欠驱动臂机械控制装置,采用计算机与板卡直接相连的方式,受到接线方式的限制,无法实现远程控制,也无法实现多台计算机共同控制一台设备,导致实验室的组建成本过高,造成资源的浪费。
发明内容
针对现有装置及方法的不足,本发明提出一种欠驱动机械臂控制装置及控制方法,以达到实时远程控制、节约成本的目的。本发明的技术方案是这样实现的:包括上位机、AD采集卡、调理模块,调理模块包括信号调理电路、数字IO调理电路、AD信号采样调理电路,所述控制装置还包括嵌入式单板机、计数采集卡、驱动接口模块和被控对象模块,所述的调理模块还包括编码器输入调理电路、PWM输出调理电路,驱动接口模块由电机驱动接口电路组成,所述的被控对象模块包括:编码器、电机、主动臂和欠驱动臂,其连接关系如下:上位机的网卡输入输出端连接嵌入式单板机的网卡输入输出端,嵌入式单板机的第一输入输出端连接计数采集卡的输入输出端,嵌入式单板机的第二输入输出端连接AD采集卡的第一输入输出端,计数采集卡的第一输入端连接第一信号调理电路的输出端,计数采集卡的第二输入端连接第二信号调理电路的输出端,第一信号调理电路的输入端连接编码器输入调理电路的第一输出端,第二信号调理电路的输入端连接编码器输入调理电路的第二输出端,计数采集卡的输出端连接PWM输出调理电路的第一输入端,AD采集卡的输出端连接PWM输出调理电路的第二输入端,AD采集卡的第二输入输出端连接数字IO调理电路的输入输出端,AD采集卡的输入端连接AD信号采样调理电路的输出端,PWM输出调理电路的第一输出端连接第一电机驱动接口电路的输入端,PWM输出调理电路的第二输出端连接第二电机驱动接口电路的输入端;编码器输入调理电路的第一输入端连接第二编码器的输出端,编码器输入调理电路的第二输入端连接第一编码器的输出端,电机驱动接口电路的输出端连接电机的输入端;所述的被控对象模块,其连接关系为:电机固定在电机基座上,电机轴的一端连接主动臂的一端,电机轴的另一端连接第一编码器,所述的主动臂与欠驱动臂通过轴承连接,在所述轴承的一端连接所述主动臂的另一端,在所述轴承的另一端连接第二编码器;本发明的信号传递过程如下:嵌入式单板机产生电压信号,经计数采集卡传递给PWM输出调理电路,由PWM输出调理电路产生脉冲给电机驱动接口电路,由电机驱动接口电路驱动被控对象的电机启动,电机启动后,会带动主动臂及欠驱动臂运动,此时,主动臂与欠驱动臂之间产生夹角,由编码器产生脉冲信号,输送给调理模块,调理模块对所述脉冲信号进行处理后输送给计数采集卡,计数采集卡将所述脉冲信号转换为数字信号并传递给嵌入式单板机,由嵌入式单板机根据该数字信号值发出第二轮控制命令,进而调节被控对象电机的转矩。本发明一种欠驱动机械臂控制装置的控制方法,包括以下步骤:步骤1:欠驱动机械臂控制装置发出电压信号,启动欠驱动机械臂的电机,此时,欠驱动机械臂静止于初始位置,所述的初始位置是指主动臂竖直向下,欠驱动臂与主动臂成顺时针180°角;步骤2:欠驱动机械臂的电机带动主动臂和欠驱动臂摆起,至非完全竖直平衡位置,此时欠驱动机械臂的电机所需转矩按照如下公式计算:u=u₁+u₂ntent="drawing" img-format="tif" inline="no" orientation="portrait" wi="700"/>其中,E=(θ₄+θ₅)g P = θ 2 θ 3 sin ( q 2 ) ( q · 1 + q · 2 ) 2 + 1 2 θ 3 2 sin ( 2 q 2 ) q · 1 2 - θ 2 θ 4 g cos ( q 1 ) + θ 3 θ 5 g cos ( q 2 ) cos ( q 1 + q 2 ) ]]> q ~ 1 = q 1 - q d ]]>h₂=(θ₄+θ₅)g+δ;u₂=u₂₁+u₂₂其中, u 21 = Σ j = 1 l [ ω ^ j s ( Σ i = 1 n v ^ ji z i + θ vj ) ] + θ w ]]> u 22 = - σ · q · 1 | q · 1 | , q · 1 ≠ 0 0 , q · 1 = 0 ]]>式中,u表示欠驱动机械臂的驱动电机输出转矩;u₁表示转矩输入的能量分量;u₂表示转矩输入的摩擦补偿分量;q₁表示主动臂从竖直向下的初始位置沿着逆时针方向所成的实际角度值;ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="47"/>表示主动臂从竖直向下的初始位置沿着逆时针方向所成的实际角速度值;q₂表示欠驱动臂相对于主动臂转动的实际角度值;ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="47"/>表示欠驱动臂相对于主动臂转动的实际角速度值;q_d表示主动臂达到非完全竖直平衡位置的目标角度,即与竖直向下位置逆时针方向成195°角;δ表示任意小正数;g表示重力加速度;θ₂,θ₃,θ₄,θ₅表示欠驱动机械臂摆臂常数;h₁,k₀,k₁,k₂,k₃,k₄表示待定正常数;ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="44"/>表示正整数且ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="143"/>μ,ζ表示两个互质正整数且μ<ζ<10,μ和ζ;u₂₁表示神经网络补偿量;u₂₂表示补偿控制量;z_i表示BP神经网络的输入,z_i∈Rⁿ;θ_vj表示BP神经网络输入层的阈值;v_ji表示BP神经网络的输入层到隐含层的权值,v_ji∈R^n×l;ω_j表示BP神经网络隐含层到输出层的权值,ω_j∈R^l;ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="160"/>表示理想的常量权值;ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="170"/>分别表示ntent="drawing" img-format="tif" inline="yes" orientation="portrait" wi="160"/>的估计值;θ_w表示BP神经网络隐含层的阈值;n表示BP神经网络输入层神经元个数;l表示BP神经网络隐含层神经元个数,l≥1;s(.)表示BP神经网络隐含层函数;ε表示BP神经网络的建模误差;σ表示正常数,且σ>|ε|;所述的非完全竖直平衡位置是指主动臂从竖直向下的初始位置沿逆时针方向旋转到达的位置,相应形成的角度,记为q₁,欠驱动臂竖直向上,所述的q₁角的范围为:135°~225°;步骤3:通过编码器反馈的角度值判断欠驱动臂是否偏离预定的非完全竖直平衡位置,如偏离,则调整欠驱动臂回到预定平衡位置,并保持维持该状态,此时欠驱动机械臂的电机所需的转矩为: u = Σ i = 1 r λ i ( z ) { K i x + L i w } Σ i = 1 r λ i ( z ) ]]>其中: L i = Γ - K i Π , Π = x w , Γ = u w ]]>式中,u表示欠驱动机械臂的驱动电机对摆臂的转矩输入;x表示欠驱动机械臂摆臂的角度和角速度状态量;w表示欠驱动机械臂非完全竖直平衡位置的参考角度信号;K_i表示欠驱动机械臂非完全竖直平衡控制调节律的状态反馈增益系数;L_i表示欠驱动机械臂非完全竖直平衡位置的参考角度信号权重调节系数;r表示非完全竖直平衡控制调节律的规则个数;λ_i表示非完全竖直平衡控制调节律的隶属度函数;z表示非完全竖直平衡控制调节律的条件变量集合;步骤4:按步骤3计算出的欠驱动机械臂的电机转矩调整电机,使摆臂调整回预定的非完全竖直平衡位置,所述的预定的非完全竖直平衡位置由使用者自行设定,角度q₁范围为135°~225°,欠驱动臂竖直向上。发明优点:本发明一种欠驱动机械臂控制装置,实现了上位机与嵌入式单板机的Internet连接,其通讯便捷且接口丰富,实现了对被控对象的实时控制,且支持多种语言编译。
附图说明
图1为本发明一种欠驱动机械臂控制装置结构框图;图2为本发明一种欠驱动机械臂控制装置被控对象模块结构示意图;图3为本发明一种欠驱动机械臂控制装置计数采集卡、AD采集卡与接口模块的连接原理图;图4为本发明一种欠驱动机械臂控制装置编码器输入调理电路的电路原理图;图5为本发明一种欠驱动机械臂控制装置驱动接口模块与PWM输出调理电路连接的电路原理图;图6为本发明一种欠驱动机械臂控制装置数字IO调理电路的电路原理图;图7为本发明一种欠驱动机械臂控制装置AD信号采样调理电路的电路原理图;图8为本发明一种欠驱动机械臂控制装置主动臂与欠驱动臂摆动示意图;图9为本发明一种欠驱动机械臂控制装置的控制方法流程图;图中,1嵌入式单板机,2计数采集卡,3AD采集卡,4调理模块,5驱动接口模块,6被控对象模块,7上位机,4-1第一信号调理电路,4-2第二信号调理电路,4-3编码器输入调理电路,4-4PWM输出调理电路,4-5数字IO调理电路,4-6AD信号采样调理电路,5-1第一电机驱动接口电路,5-2第二电机驱动接口电路,6-1电机基座,6-2电机,6-3主动臂,6-4欠驱动臂,6-5第一编码器,6-6第二编码器。
具体实施方式
下面结合附图和实施例对本发明作进一步详细说明。一种欠驱动机械臂控制装置,其结构框图如图1所示,其中,被控对象模块的结构示意图如图2所示,本实施例中,各部件的型号如下:嵌入式单板机的型号为SBCEC3-1621CLDNA,计数采集卡的型号为ART2008,AD采集卡的型号为ART2010,信号调理电路采用芯片实现,其型号为74LS74;其中,计数采集卡ART2008由第一计数器82C52接口电路、第一数字IO接口电路和第二计数器82C52接口电路组成;AD采集卡由第三计数器82C54、可编程并行接口电路8255A和AD接口组成;电机驱动接口电路的型号为L298;PWM输出调理电路的型号为Header5X2。如图3所示,为嵌入式单板机与计数采集卡、AD采集卡及信号调理电路相连接的电路原理图,其中,嵌入式单板机SBCxa0EC3-1621CLDNA的PC104端连接计数采集卡ART2008的PC104端,采用PC/104总线方式进行通讯,嵌入式单板机SBCxa0EC3-1621CLDNA的PC104端连接AD采集卡ART2010的PC104端,采用PC/104总线方式进行通讯;计数采集卡ART2008中第一计数器2008XS1的9端连接信号调理电路1的CLK1端,计数采集卡ART2008中第一计数器2008XS1的2、6、8、28、32端同时连接信号调理电路1的/Q1端、GND端及芯片2010CN2的10、14端,计数采集卡ART2008中第一计数器2008XS1的1、18端分别连接信号调理电路2的CLK1、CLK2端,计数采集卡ART2008中第一计数器2008XS1的20脚连接第二芯片2010CN2的1号脚,计数采集卡ART2008中第一计数器2008XS1的PWM1和PWM2端依次连接PWM输出调理电路的PWM1和PWM2端,计数采集卡ART2008中第一计数器2008XS1的30端连接时钟芯片CLK8M的2号脚;计数采集卡ART2008中第一计数器2008XS1的4号脚连接第一74LS74的CLK2脚,计数采集卡ART2008中第一计数器2008XS1的17、29、31、33脚同时连接2008XS2的2、4、6、8、10、15、17、19脚,计数采集卡ART2008中第一计数器2008XS1的10、13、19、22、27脚同时连接2010CN2的2、5、8脚,再与CLK8M的1号脚相连,计数采集卡ART2008中第一计数器2008XS1的PWM1端、PWM2端依次连接PWM输出调理电路的PWM1和PWM2端;计数采集卡ART2008中第二计数器2008XS2的11脚连接信号调理电路1的Q1脚,计数采集卡ART2008中第二计数器2008XS2的13脚连接信号调理电路2的Q1脚,计数采集卡ART2008中第二计数器2008XS2的12脚连接信号调理电路1的Q2脚,计数采集卡ART2008中第二计数器2008XS2的14脚连接信号调理电路2的Q2脚,计数采集卡ART2008中第二计数器2008XS2的1、3、5、7、9脚接地GND端,计数采集卡ART2008中第二计数器2008XS2的16、18、20端同时连接AD采集卡2010CN1芯片的2、4、6、8、10、12、14、16、18、20、37~50端和AD采集卡2010CN2芯片的11、13端,再与信号调理电路2的/Q1、GND、/Q2端相连,并同时连接信号调理电路1的/Q2端;AD采集卡2010CN2芯片的PWM3端、PWM4端依次连接PWM输出调理电路的PWM3和PWM4端,AD采集卡2010CN2的12端连接时钟芯片CLK8M的3号脚;编码器输入调理电路与信号调理电路的连接如图4所示,信号调理电路1的CH0A端、CH0B端依次连接编码器输入调理电路的CH0A端、CH0B端,信号调理电路1的CH1A端、CH1B端依次连接编码器输入调理电路的CH1A端、CH1B端,编码器输入调理电路设有8个接线端子,其中的4个接线端子连接第一编码器,另外4个接线端子连接第二编码器;本实施例中采用2个电机驱动接口电路与PWM输出调理电路相连接,如图5所示,电机驱动接口电路1和电机驱动接口2,其与PWM输出调理电路Header5X2的连接关系为:Header5X2的9、10脚同时连接电机驱动接口2的1、2、3、10、11、18~20脚,再接地,Header5X2的2脚连接电机驱动接口2的EnableB脚,Header5X2的4脚电机驱动接口2的EnableA脚,Header5X2的6脚连接电机驱动接口1的EnableB脚,Header5X2的8脚电机驱动接口1的EnableA脚,电机驱动接口电路1中接线端口M1连接被控对象电机的正负极;数字IO调理电路的电路原理图如图6所示,其与AD采集卡得连接关系如下:数字IO调理电路的DO0~DO7脚依次连接AD采集卡的DO0~DO7脚,数字IO调理电路的DI0~DI7依次连接AD采集卡的DI0~DI7脚;图7为AD信号采样调理电路的电路原理图,其与AD采集卡的连接关系如下:AD信号采样调理电路的ACH0~ACH7脚依次连接AD采集卡的ACH0~ACH7脚,AD信号采样调理电路的第一AGND脚、第二AGND脚依次连接AD采集卡的第一AGND脚和第二AGND脚本实施例采用欠驱动机械臂控制装置的控制方法,如图8和图9所示,包括以下步骤:步骤1:欠驱动机械臂控制装置发出电压信号,启动欠驱动机械臂的电机,此时,欠驱动机械臂静止于初始位置,所述的初始位置是指主动臂竖直向下,欠驱动臂与主动臂成顺时针180°角;步骤2:欠驱动机械臂的电机带动主动臂和欠驱动臂摆起,至非完全竖直平衡位置,此时欠驱动机械臂的电机所需转矩按照如下公式计算:u=u₁+u₂ntent="drawing" img-format="tif" inline="no" orientation="portrait" wi="700"/>其中,E=(θ₄++₅)g P = θ 2 θ 3 sin ( q 2 ) ( q · 1 + q · 2 ) 2 + 1 2 θ 3 2 sin ( 2 q 2 ) q · 1 2 - θ 2 θ 4 g cos ( q 1 ) + θ 3 θ 5 g cos ( q 2 ) cos ( q 1 + q 2 ) ]]> q ~ 1 = q 1 - q d ]]>h₂=(θ₄+θ₅)g+δ;u₂=u₂₁+u₂₂其中, u 21 = Σ j = 1 l [ ω ^ j s ( Σ i = 1 n v ^ ji z i + θ vj ) ] + θ w ]]> u 22 = - σ · q · 1 | q · 1 | , q · 1 ≠ 0 0 , q · 1 = 0 ]]>步骤3:通过编码器反馈的角度值判断欠驱动臂是否偏离预定的非完全竖直平衡位置,如偏离,则调整欠驱动臂回到预定平衡位置,并保持维持该状态,此时欠驱动机械臂的电机所需的转矩为: u = Σ i = 1 r λ i ( z ) { K i x + L i w } Σ i = 1 r λ i ( z ) ]]>其中: L i = Γ - K i Π , Π = x w , Γ = u w ]]>步骤4:按步骤3计算出的欠驱动机械臂的电机转矩调整电机,使摆臂调整回预定的非完全竖直平衡位置,所述的预定的非完全竖直平衡位置由使用者自行设定,角度范围为q₁角为135°~225°,欠驱动臂竖直向上。